/*
 * PID_controller.c
 *
 * Created: 5/9/2015 11:13:43 AM
 *  Author: David Hunt
 */ 

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#define P_GAIN 0.8
#define I_GAIN 0.005
#define D_GAIN 0.01
#define F_CPU 1000000UL

int main(void)
{	
int set_rpm = 0;
int previous_error = 0;
int error;
int feedback_rpm;
int output;
int D_error = 0;
int I_error = 0;

void init_io(){
	
	PORTB = 0x06;	// PortB1 and PortB2 are input; PinB0 in input
	
	// Fast PWM mode - 16bit , Fixed frequency , Non- Inverting mode , TOP = ICR1 = 259
	// Frequency = F_CPU/(N*(1+TOP) = 60Hz , N = 64
	// Duty cycle set by OCR1A and OCR1B
	TCCR1A = (1 << COM1A1) | (1 << WGM11);
	TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS10) | (1 << CS11);
	ICR1 = 259;
	TCNT1L = 0;
	TCNT1H = 0;
	OCR1A = 0;
	
	//Setup timer0 as a counter for encoder data
	TCCR0B = (1<<CS02) | (1<<CS01) | (1<<CS00);
	TCNT0=0;
	
}

void usart_init (void)
{
	UCSR0B = (1<<TXEN0);
	UCSR0C = (1<<UCSZ01) | (1<<UCSZ00);
	UBRR0L = 0x33;
}

int main(void){

	usart_init();
	init_io();

	int sample1, sample2;
	previous_error = set_rpm - feedback_rpm;
	
	//Start infinite loop
	while(1){
		
		//RECIEVE DATA
		while (! (UCSR0A & (1<<RXC0)));
			set_rpm = UDR0;
				
		error = set_rpm - feedback_rpm;
		I_error += (error);
		D_error = (error - previous_error);
		output = (P_GAIN * error) + (I_GAIN * I_error) + (D_GAIN * D_error);
		previous_error = error;

		if(OCR1A + output > 259)
			OCR1A= 259;
		else if(OCR1A + output < 0)
				OCR1A= 0;
			else
				OCR1A += output;
					
			sample1=TCNT0;
			_delay_ms(1);
			sample2=TCNT0;
														
			feedback_rpm = (sample2-sample1)*3000;
					
		//TRANSMIT CODE
		while (!(UCSR0A & (1<<UDRE0)));
			UDR0 = feedback_rpm;
}}}